#include "map_download_action_client.h"
#include <QMetaType>
#include <QString>
#include <QDir>
#include <QFile>
#include <QDebug>
MapDownloadActionClient::MapDownloadActionClient(const rclcpp::NodeOptions & node_options)
: QObject(), rclcpp::Node("map_download_action_client", node_options)
{
  // 注册Qt类型，如果需要的话
  qRegisterMetaType<quint32>("quint32");
  
  // 初始化客户端指针为空，将在请求时根据robotId创建
  this->client_ptr_ = nullptr;
}

bool MapDownloadActionClient::is_server_ready() const
{
  if (!this->client_ptr_) {
    return false;
  }
  return this->client_ptr_->wait_for_action_server(std::chrono::seconds(5));
}


void MapDownloadActionClient::onRequestDownloadMap(const QString &robotId, const QString &mapName)
{
  currentRobotId = robotId;
  currentMapName = mapName;
  qDebug() << "Request download map:" << robotId << mapName;
  
  // 根据robotId构建action名称
  QString actionName = QString("/%1/map_download").arg(robotId);
  
  // 如果客户端已经存在，先重置它
  if (this->client_ptr_) {
    this->client_ptr_.reset();
  }
  
  // 创建新的action客户端
  this->client_ptr_ = rclcpp_action::create_client<MapDownload>(
    this->get_node_base_interface(),
    this->get_node_graph_interface(),
    this->get_node_logging_interface(),
    this->get_node_waitables_interface(),
    actionName.toStdString());

  // 增加重试机制
  int retry_count = 0;
  const int max_retries = 3;
  while (retry_count < max_retries && !this->is_server_ready()) {
    RCLCPP_WARN(this->get_logger(), "Action server not available, retrying... (%d/%d)", retry_count + 1, max_retries);
    rclcpp::sleep_for(std::chrono::seconds(1));
    retry_count++;
  }

  if (!this->is_server_ready()) {
    RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting and retries");
    emit downloadCompleted(false, QString::fromStdString("Action server not available"));
    return;
  }

  // 直接发送目标
  send_goal(mapName.toStdString());
}


void MapDownloadActionClient::send_goal(const std::string &map_name)
{
  using namespace std::placeholders;
  // 初始化接收块跟踪向量
  totalChunks = 0;
  receivedChunks.clear();
  
  auto goal_msg = MapDownload::Goal();
  goal_msg.map_name = map_name;

  RCLCPP_INFO(this->get_logger(), "Sending goal for map: %s", map_name.c_str());
  emit downloadFeedback(QString("Sending goal for map: %1").arg(QString::fromStdString(map_name)));

  auto send_goal_options = rclcpp_action::Client<MapDownload>::SendGoalOptions();
  // 设置目标响应回调函数，用于处理服务器对目标的接受或拒绝
  send_goal_options.goal_response_callback =
    std::bind(&MapDownloadActionClient::goal_response_callback, this, _1);
  // 设置反馈回调函数，用于处理服务器发送的执行进度反馈
  send_goal_options.feedback_callback =
    std::bind(&MapDownloadActionClient::feedback_callback, this, _1, _2);
  // 设置结果回调函数，用于处理服务器发送的最终执行结果
  send_goal_options.result_callback =
    std::bind(&MapDownloadActionClient::result_callback, this, _1);
  
  RCLCPP_INFO(this->get_logger(), "About to send goal with callbacks registered");
  // 异步发送目标到服务器
  this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}

void MapDownloadActionClient::goal_response_callback(const GoalHandleMapDownload::SharedPtr & goal_handle)
{
  if (!goal_handle) {
    RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    emit downloadCompleted(false, QString("Goal was rejected by server"));
  } else {
    RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
    emit downloadFeedback(QString("Goal accepted by server, waiting for result"));
  }
}

void MapDownloadActionClient::feedback_callback(
  GoalHandleMapDownload::SharedPtr,
  const std::shared_ptr<const MapDownload::Feedback> feedback)
{
  RCLCPP_INFO(
    this->get_logger(),
    "Feedback: %u/%u chunks sent, %.1f%% complete, chunk size: %u",
    feedback->chunks_sent,
    feedback->total_chunks,
    feedback->percentage,
    feedback->chunk_size);
  
    qDebug() << "Feedback: " << feedback->chunks_sent << "/" << feedback->total_chunks << " chunks sent, " << feedback->percentage << "% complete, chunk size: " << feedback->chunk_size;
    
  // 更新总数据块数
  if (totalChunks == 0 && feedback->total_chunks > 0) {
    totalChunks = feedback->total_chunks;
    receivedChunks.resize(totalChunks, false);
  }
  
  // 标记当前数据块为已接收
  if (feedback->chunks_sent > 0 && feedback->chunks_sent <= totalChunks) {
    receivedChunks[feedback->chunks_sent - 1] = true;
  }
  
  // 确保文件已打开
  if (!mapFile.isOpen()) {
    QString mapDirPath = QString("data/%1/map").arg(currentRobotId);
    QDir mapDir(mapDirPath);
    if (!mapDir.exists()) {
      mapDir.mkpath(".");
    }
    
    QString mapFileName = currentMapName;
    if (!mapFileName.endsWith(".smap")) {
      mapFileName += ".smap";
    }
    
    QString mapFilePath = QString("%1/%2").arg(mapDirPath, mapFileName);
    mapFile.setFileName(mapFilePath);
    
    // 打开文件并预分配空间
    if (mapFile.open(QIODevice::ReadWrite)) { // 使用ReadWrite模式
      // 预分配文件空间，确保文件大小足够容纳所有数据块
      if (totalChunks > 0) {
        // 估算文件大小：(总块数-1) * 262144 + 最后一块大小（这里假设最大262144）
        qint64 estimatedSize = totalChunks * 262144;
        mapFile.resize(estimatedSize);
      }
      RCLCPP_INFO(this->get_logger(), "Opened file for writing: %s", mapFilePath.toStdString().c_str());
    } else {
      RCLCPP_ERROR(this->get_logger(), "Failed to open file for writing: %s", mapFilePath.toStdString().c_str());
      return; // 打开文件失败则直接返回
    }
  }
  
  // 写入数据块
  if (mapFile.isOpen() && feedback->chunk_data.size() > 0) {
    QByteArray data(reinterpret_cast<const char*>(feedback->chunk_data.data()), feedback->chunk_data.size());
    
    // 根据数据块索引计算写入位置（每个块固定大小262144字节，除了最后一个块）
    qint64 writePosition = (feedback->chunks_sent - 1) * 262144;
    mapFile.seek(writePosition);
    
    qint64 bytesWritten = mapFile.write(data);
    if (bytesWritten != data.size()) {
      RCLCPP_ERROR(this->get_logger(), "Failed to write complete data to file. Expected: %d, Written: %ld", 
                   static_cast<int>(data.size()), bytesWritten);
    } else {
      RCLCPP_INFO(this->get_logger(),"Written chunk %u/%u to file, size: %d bytes, file size: %ld bytes", 
                   feedback->chunks_sent, feedback->total_chunks, static_cast<int>(data.size()), mapFile.size());
    }
    
    // 检查是否所有数据块都已接收，如果是则关闭文件
    if (totalChunks > 0) {
      bool allReceived = true;
      for (bool received : receivedChunks) {
        if (!received) {
          allReceived = false;
          break;
        }
      }
      
      if (allReceived) {
        // 调整文件大小到实际大小
        // 计算实际大小：前(n-1)块 * 262144 + 最后一块的实际大小
        // 由于我们无法提前知道每块的大小，使用一个保守的估计
        mapFile.flush();
        mapFile.close();
        RCLCPP_INFO(this->get_logger(), "Closed map file after writing all chunks. Final file size: %ld bytes", 
                    mapFile.size());
      }
    }
  } else if (feedback->chunk_data.size() > 0) {
    RCLCPP_WARN(this->get_logger(), "File not open but chunk data received, size: %d", 
                static_cast<int>(feedback->chunk_data.size()));
  } else {
    RCLCPP_INFO(this->get_logger(), "Received empty chunk %u/%u", 
                feedback->chunks_sent, feedback->total_chunks);
  }
  
  emit downloadProgress(
    static_cast<int>(feedback->percentage),
    feedback->chunks_sent,
    feedback->total_chunks);
}


void MapDownloadActionClient::result_callback(const GoalHandleMapDownload::WrappedResult & result)
{
  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
      emit downloadCompleted(false, QString("Goal was aborted"));
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
      emit downloadCompleted(false, QString("Goal was canceled"));
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
      emit downloadCompleted(false, QString("Unknown result code"));
      return;
  }

  QString message = QString::fromStdString(result.result->message);
  
  // 处理下载结果并发出完成信号
  if (result.result->success) {
    RCLCPP_INFO(this->get_logger(), "Map download succeeded: %s Total chunks received: %u", result.result->message.c_str(),result.result->total_chunks);
    emit downloadCompleted(true, message);
  } else {
    RCLCPP_ERROR(this->get_logger(), "Map download failed: %s", result.result->message.c_str());
    emit downloadCompleted(false, message);
  }
}